# -*- coding: utf-8 -*-
import math
                # Чтение файла, формирование списков данных
f=open('./X_US_IR','r') #Открываем файл с данными в режиме чтения  
l=[] #список длин, соответствующих измерениям
us=[] #список измерений ультразвукового дальномера после фильтра
ir=[] #список измерений инфракрасного дальномера после фильтра
p=10 #порядок КИХ-фильтра, реализующего скользящее среднее
rough_us=[]
rough_ir=[]
a=f.readline() #предварительное присвоение, а - строковая переменная, содержит первую строку файла телеметрии
while (a != "\n"): #условный цикл, выполняющийся до тех пор, пока не достигнут конец файла
  b=a.split() # b = список из элементов (строковых), разделявшихся пробелами в а 
  #print b
  #print int(b[1])
  a=f.readline()
  l.append(float(b[0])) # дописываем в список длин целочисленное значение текущей длины
  rough_us.append(float(b[1])) # дописываем в список показаний ультразвукового дальномера текущее
  rough_ir.append(float(b[2])) # дописываем в список показаний инфракрасного дальномера текущее
f.close() # закрываем файл.
f=open('./sw_out.txt','w') #Открываем файл для записи выходных данных и отладочной информации 
#print l
#print rough_ir
#print rough_us
#print len(l), len(rough_ir), len(rough_us)

                #   Предварительная обработка списков данных
#Функция, реализующая КИХ-фильтр порядка p, реализующий скользящее среднее для списка данных x
def sw(x,p):    
  a=x   #   вводим дополнительную переменную, которая содержит входной список
  y=[]    #инициализируем выходной список
  avg = 0.    #   среднее из текущего и p предыдущих элементов списка
  window = p    #   порядок фильтра
  for i, c in enumerate(a):
    if i<window:    #   элементы с номером меньшим, чем порядок фильтра, не изменяются
      avg+=c/window   #   однако вносят свой вклад в среднее значение
      y.append(c)
      test_string= 'rough_' + str(i) + ' = ' + str(a[i]) +' smooth_' + str(i) + ' = ' + str(y[i]) + '\n'
      f.write(test_string)
      continue
    avg-=a[i-window]/window 
    avg+=c/window   
    y.append(avg)   
  #  print i, avg, c, i-window
    test_string= 'rough_' + str(i) + ' = ' + str(a[i]) +' smooth_' + str(i) + ' = ' + str(y[i]) + '\n'
    f.write(test_string)
  return y

    #Обработка выходных данных фильтром
f.write('ir:\n')
ir=sw(rough_ir,p)
f.write('us:\n')
us=sw(rough_us,p)

    #Функция. Интерполяция методом наименьших квадратов
def mnk(x,y): #в качестве входных параметров принимает два списка (аргументов и значений интерполируемой функции)
  if len(x) != len (y):
    print "mnk_error: dimension mismatch"
  a=x 
  sx=0.
  sy=0.
  sx2=0.
  sxy=0.
  n=len(a)
  b=y   #дополнительные переменные, содержащие входные аргументы
  for j, c in enumerate(a):
    sx+=c
    sy+=b[j]
    sx2+=c*c
    sxy+=c*b[j]
#  print sx
#  print sy
#  print sx2
#  print sxy
  A_0=(sy*sx2-sxy*sx)/(n*sx2-sx*sx)
  A_1=(n*sxy - sx*sy)/(n*sx2-sx*sx)
  for j in enumerate a
  return [A_0,A_1]    #   функция возвращает список, состоящий из коэффициентов интерполянта

    #интерполяция данных
koef_ir=mnk(ir,l)   #   коэффициенты интерполяции для инфракрасного дальномера (после фильтра)
koef_rough_ir=mnk(rough_ir,l) #   коэффициенты интерполяции для инфракрасного дальномера (до фильтра)

def mnk_count(koef, sensor_data):   #   функция, считающая значения дистанции до цели по показаниям датчика
# ф-я принимает список коэффициентов интерполяции и список показаний датчика
  sen=sensor_data
  A_0=koef[0]
  A_1=koef[1]
  dist=[]
  for d in sensor_data:
    dist.append (A_0+A_1*d)
  return dist   #Ф-я фозвращает список значений дистанций до цели, посчитанных по показаниям датчика
  
def mnk_fitness(dist,ref_dist):       #Функция оценки качества интерполяции
#Функция принимает список эталонных значений дистанций и список дистанций, посчитанных по показаниям датчиков
  a=ref_dist
  b=dist
  dif_dist=0.
  n=len(a)
  for i, d in enumerate a
    dif_sq+=(d-b[i])*(d-b[i])/n
    ave_sq+=d*d
  preciseness=dif_sq/ave_sq*100
  return diff_sq #Функция вовзращает среднее относительное отклонение от среднего значения определённой длины в % 
